"""
创建一个Request的发布对象
创建twist订阅对象
订阅者的回调函数中实现消息转换并发布Request
"""
import rclpy
from rclpy.node import Node
from unitree_api.msg import Request
from geometry_msgs.msg import Twist
from .sport_model import ROBOT_SPORT_API_IDS
import json

class TwistBridge(Node):
    def __init__(self):
        super().__init__('twist_bridge') #节点名称
        self.get_logger().info("Twist消息即将桥接转化为Request消息")
        self.rquest_pub = self.create_publisher(Request,"/api/sport/request",10)#消息类型,话题名称，队列消息
        self.twist_sub = self.create_subscription(Twist,"cmd_vel",self.twist2request,10)#消息类型，话题名称，回调函数    
    def twist2request(self,twist: Twist):
        request =Request()
        api_id = ROBOT_SPORT_API_IDS["BALANCESTAND"] #平衡站立
        #转换（x\y方向的线速度以及绕z轴的角速度）
        x = twist.linear.x
        y = twist.linear.y
        z = twist.angular.z
        #设置api_id
        
        if x != 0 or y != 0 or z != 0:
            api_id = ROBOT_SPORT_API_IDS["MOVE"]
            #MOVE模式必须设置参数
            js = {"x":x ,"y":y ,"z":z }
            request.parameter = json.dumps(js)#强制转换
            #self.get_logger().info(f"当前速度：{request.parameter}")
            
            request.header.identity.api_id= api_id
            self.rquest_pub.publish(request)
def main(args=None):
    rclpy.init(args=args)
    rclpy.spin(TwistBridge())
    rclpy.shutdown()
if __name__ == '__main__':
    main()

